#include "sensor.h"
#include <QGlobalStatic>
#ifndef Q_OS_WASM
#include <QSensor>
#include <QSensorReading>
#include <QSensorManager>

#include <QAccelerometer     > 
#include <QGyroscope         > 
#include <QMagnetometer      > 
#include <QCompass           > 
#include <QOrientationSensor > 
#include <QRotationSensor    > 
#include <QTiltSensor        > 
#include <QLightSensor       > 
#include <QAmbientLightSensor> 
#include <QProximitySensor   >
#include <QGeoPositionInfoSource>
#endif
#include <QDebug>
#include "mavlink/selfio/mavlink.h"
#define sys_id 1
#define comp_id 1
Q_GLOBAL_STATIC(Sensor, sensorInterface)
Sensor *Sensor::instance()
{
    return sensorInterface;
}

Sensor::Sensor(QObject *parent)
    : QObject{parent}
{
   // return;
    #ifndef Q_OS_WASM
    m_accel             =new QAccelerometer      (this);
    m_gyro              =new QGyroscope          (this);
    m_magnetometer      =new QMagnetometer       (this);
    m_compass           =new QCompass            (this);
    m_orientationSensor =new QOrientationSensor  (this);
    m_rotationSensor    =new QRotationSensor     (this);
    m_tiltSensor        =new QTiltSensor         (this);
    m_lightSensor       =new QLightSensor        (this);
    m_ambientLightSensor=new QAmbientLightSensor (this);
    m_proximitySensor   =new QProximitySensor    (this);
    gps = QGeoPositionInfoSource::createDefaultSource(this);
    enLight();
    return;
    m_accel             ->setDataRate(100);
    m_gyro              ->setDataRate(100);
    m_magnetometer      ->setDataRate(100);
    m_compass           ->setDataRate(100);
    m_orientationSensor ->setDataRate(100);
    m_rotationSensor    ->setDataRate(100);
    m_tiltSensor        ->setDataRate(100);

    m_ambientLightSensor->setDataRate(100);
    m_proximitySensor   ->setDataRate(100);
    if(gps){
        gps->setUpdateInterval(100); // 1秒更新一次
        gps->startUpdates();
    }

    // 加速度计

    m_accel             ->setAccelerationMode(QAccelerometer::User);
#ifdef Q_OS_ANDROID
    m_accel             ->setIdentifier(QSensor::sensorsForType(QAccelerometer     ().type()).first());
    m_gyro              ->setIdentifier(QSensor::sensorsForType(QGyroscope         ().type()).first());
    m_magnetometer      ->setIdentifier(QSensor::sensorsForType(QMagnetometer      ().type()).first());
    m_compass           ->setIdentifier(QSensor::sensorsForType(QCompass           ().type()).first());
    m_orientationSensor ->setIdentifier(QSensor::sensorsForType(QOrientationSensor ().type()).first());
    m_rotationSensor    ->setIdentifier(QSensor::sensorsForType(QRotationSensor    ().type()).first());
    m_tiltSensor        ->setIdentifier(QSensor::sensorsForType(QTiltSensor        ().type()).first());
    m_ambientLightSensor->setIdentifier(QSensor::sensorsForType(QAmbientLightSensor().type()).first());
    m_proximitySensor   ->setIdentifier(QSensor::sensorsForType(QProximitySensor   ().type()).first());
    m_accel->setActive(true);
    qInfo()<<"加速度计开始："<<m_accel->start();
#endif
    connect(m_accel, &QAccelerometer::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
        qDebug()<<QString("加速度 x:%1 y:%2 z:%3")
                        .arg(m_accel->reading()->x())
                        .arg(m_accel->reading()->y())
                        .arg(m_accel->reading()->z());
        }
        auto r = m_accel->reading();
        m_accelX = r->x();
        m_accelY = r->y();
        m_accelZ = r->z();
        emit accelChanged();

        mavlink_message_t msg;
        mavlink_accelerometer_t mb;
        mb.ACC_X = r->x();
        mb.ACC_Y = r->y();
        mb.ACC_Z = r->z();
        mavlink_msg_accelerometer_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    // 陀螺仪
    connect(m_gyro, &QGyroscope::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("陀螺仪 x:%1 y:%2 z:%3")
                  .arg(m_gyro->reading()->x())
                  .arg(m_gyro->reading()->y())
                  .arg(m_gyro->reading()->z());
        }

        auto r = m_gyro->reading();
        m_gyroX = r->x();
        m_gyroY = r->y();
        m_gyroZ = r->z();
        emit gyroChanged();

        mavlink_message_t msg;
        mavlink_gyroscope_t mb;
        mb.GYRO_X = r->x();
        mb.GYRO_Y = r->y();
        mb.GYRO_Z = r->z();
        mavlink_msg_gyroscope_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    m_gyro->setActive(true);

    // 磁力计
    connect(m_magnetometer, &QMagnetometer::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
         flag = true;
        qDebug()<<QString("磁力计 x:%1 y:%2 z:%3")
                  .arg(m_magnetometer->reading()->x())
                  .arg(m_magnetometer->reading()->y())
                  .arg(m_magnetometer->reading()->z());
        }
        auto r = m_magnetometer->reading();
        m_magX = r->x();
        m_magY = r->y();
        m_magZ = r->z();
        emit magnetometerChanged();
        mavlink_message_t msg;
        mavlink_magnetometer_t mb;
        mb.MAG_X = r->x();
        mb.MAG_Y = r->y();
        mb.MAG_Z = r->z();
        mavlink_msg_magnetometer_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    m_magnetometer->setActive(true);
    if (!m_magnetometer->isActive()) {
        qWarning() << "磁力计启动失败！";
    }

    // 罗盘
    connect(m_compass, &QCompass::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("罗盘方向:%1").arg(m_compass->reading()->azimuth());
        }
        auto r = m_compass->reading();
        m_compassAzimuth = r->azimuth();
        emit compassChanged();

        mavlink_message_t msg;
        mavlink_compass_t mb;
        mb.Compass = r->azimuth();
        mavlink_msg_compass_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    m_compass->setActive(true);

    // 方向
    connect(m_orientationSensor, &QOrientationSensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("方向:%1").arg(m_orientationSensor->reading()->orientation());
        }
        auto r = m_orientationSensor->reading();
        m_orientation = r->orientation();
        emit orientationChanged();

        mavlink_message_t msg;
        mavlink_orientationsensor_t mb;
        mb.Orientation = m_orientation;
        mavlink_msg_orientationsensor_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
   m_orientationSensor->setActive(true);
    qInfo()<<"方向开始："<< m_orientationSensor->start();

    // 旋转传感器
    connect(m_rotationSensor, &QRotationSensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("旋转传感器 x:%1 y:%2 z:%3")
                  .arg(m_rotationSensor->reading()->x())
                  .arg(m_rotationSensor->reading()->y())
                    .arg(m_rotationSensor->reading()->z());
       }
        auto r = m_rotationSensor->reading();
        m_rotX = r->x();
        m_rotY = r->y();
        m_rotZ = r->z();
        emit rotationChanged();

        // mavlink_message_t msg;
        // mavlink_t mb;
        // mb.Orientation = m_orientation;
        // mavlink_msg_orientationsensor_encode(sys_id, comp_id,&msg, &mb);
        // uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        // uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        // if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    m_rotationSensor->setActive(true);

    // 倾斜传感器
    connect(m_tiltSensor, &QTiltSensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("倾斜传感器 x:%1 y:%2")
                .arg(m_tiltSensor->reading()->xRotation())
                .arg(m_tiltSensor->reading()->yRotation());
        }
        auto r = m_tiltSensor->reading();
        m_tiltX = r->xRotation();
        m_tiltY = r->yRotation();
        emit tiltChanged();

        mavlink_message_t msg;
        mavlink_tiltsensor_t mb;
        mb.Tilt_X = r->xRotation();
        mb.Tilt_Y = r->yRotation();
        mavlink_msg_tiltsensor_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
    m_tiltSensor->setActive(true);
    qInfo()<<"倾斜传感器开始："<< m_tiltSensor->start();

    // 光线



    // 环境光
    connect(m_ambientLightSensor, &QAmbientLightSensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("环境光:%1").arg(m_ambientLightSensor->reading()->lightLevel());
        }
        auto r = m_ambientLightSensor->reading();
        m_ambientLight = r->lightLevel();
        emit ambientLightChanged();

        mavlink_message_t msg;
        mavlink_ambientlight_t mb;
        mb.ambientLight = r->lightLevel();
        mavlink_msg_ambientlight_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
   m_ambientLightSensor->setActive(true);


    // 距离
    connect(m_proximitySensor, &QProximitySensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("距离:%1").arg(m_proximitySensor->reading()->close());
        }
        auto r = m_proximitySensor->reading();
        m_proximity = r->close();
        emit proximityChanged();

        mavlink_message_t msg;
        mavlink_proximitysensor_t mb;
        mb.Proximity = r->close();
        mavlink_msg_proximitysensor_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));
    });
   m_proximitySensor->setActive(true);
    qInfo()<<"距离开始："<< m_proximitySensor->start();

    connect(gps, &QGeoPositionInfoSource::positionUpdated,this,
                     [=](const QGeoPositionInfo &info){
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug() << "Latitude:" << info.coordinate().latitude();
            qDebug() << "Longitude:" << info.coordinate().longitude();
        }
        mavlink_message_t msg;
        mavlink_gps_t mb;
        mb.lat = info.coordinate().latitude();
        mb.lon =info.coordinate().longitude();
        mavlink_msg_gps_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(this->_sendCallBack)this->_sendCallBack(QByteArray((char*)buffer,len));
       });



#endif
}

void Sensor::enLight()
{
#ifdef Q_OS_ANDROID
    m_lightSensor->setDataRate(1000);
    QList<QByteArray> sensors = QSensor::sensorsForType(QLightSensor().type());
    m_lightSensor->setIdentifier(sensors.first());
    connect(m_lightSensor, &QLightSensor::readingChanged, this, [&] {
        static bool flag = false;
        if(!flag){
            flag = true;
            qDebug()<<QString("光线:%1").arg(m_lightSensor->reading()->lux());
        }
        auto r = m_lightSensor->reading();
        m_lightLux = r->lux();
        emit lightChanged();


        mavlink_message_t msg;
        mavlink_lightsensor_t mb;
        mb.light = r->lux();
        mavlink_msg_lightsensor_encode(sys_id, comp_id,&msg, &mb);
        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t len = mavlink_msg_to_send_buffer(buffer,&msg);
        if(_sendCallBack)_sendCallBack(QByteArray((char*)buffer,len));

    });
    m_lightSensor->setActive(true);
#endif
}

void Sensor::setLightLux(double value)
{
    if(qFuzzyCompare(value,lightLux())){
        return;
    }

    m_lightLux = value;
    emit lightChanged();
}

void Sensor::set_sendCallBack(std::function<void (QByteArray)> ptr)
{
    _sendCallBack = ptr;
}

void Sensor::getALL()
{
    #ifndef Q_OS_WASM
    // 获取所有可用传感器类型
    auto availableSensors = QSensor::sensorTypes();
    qDebug() << "Available sensor types:";
    foreach(const auto &type , availableSensors){
        qDebug() << type;
    }

    // 创建每个传感器对象，并打印值
   foreach(const auto &type , availableSensors) {
        QSensor sensor(type);
        sensor.busyChanged();
    }
#endif
}
